package calculation;

/**
 *
 * @author Oanh
 */
public class Remez {

    public double h[];
    final int BANDPASS = 1;
    final int DIFFERENTIATOR = 2;
    final int HILBERT = 3;
    final int NEGATIVE = 0;
    final int POSITIVE = 1;
    final double Pi = Math.PI;
    final double Pi2 = 2 * Pi;
    final int GRIDDENSITY = 16;
    final int MAXITERATIONS = 40;
    public double[] E;
    public double maxE;
    public double minE;
   public   int[] Ext;

    public Remez(int hSize, int numtaps, int numband, double bands[], double des[], double weight[], int type) {
        h = new double[hSize];
        double[] Grid, W, D, E;
        int i, iter, gridsize, r;
        
        double[] taps;
        double c;
        double[] x, y, ad;
        int symmetry;

        if (type == BANDPASS) {
            symmetry = POSITIVE;
        } else {
            symmetry = NEGATIVE;
        }

        r = numtaps / 2;  
        if ((numtaps % 2 == 1) && (symmetry == POSITIVE)) {
            r++;
        }

        /*
         * Predict dense grid size in advance for memory allocation .5 is so we
         * round up, not truncate
         */
        gridsize = 0;
        for (i = 0; i < numband; i++) {
            gridsize += (int) (2 * r * GRIDDENSITY * (bands[2 * i + 1] - bands[2 * i]) + .5);
        }
        if (symmetry == NEGATIVE) {
            gridsize--;
        }

        /*
         * Dynamically allocate memory for arrays with proper sizes
         */
        Grid = new double[gridsize];
        D = new double[gridsize];
        W = new double[gridsize];
        E = new double[gridsize];


        Ext = new int[r + 1];
        taps = new double[r + 1];
        x = new double[r + 1];
        y = new double[r + 1];
        ad = new double[r + 1];

        /*
         * Create dense frequency grid
         */
        CreateDenseGrid(r, numtaps, numband, bands, des, weight,
                gridsize, Grid, D, W, symmetry);
        InitialGuess(r, Ext, gridsize);

        /*
         * For Differentiator: (fix grid)
         */
        if (type == DIFFERENTIATOR) {
            for (i = 0; i < gridsize; i++) {
                /*
                 * D[i] = D[i]*Grid[i];
                 */
                if (D[i] > 0.0001) {
                    W[i] = W[i] / Grid[i];
                }
            }
        }

        /*
         * For odd or Negative symmetry filters, alter the D[] and W[] according
         * to Parks McClellan
         */
        if (symmetry == POSITIVE) {
            if (numtaps % 2 == 0) {
                for (i = 0; i < gridsize; i++) {
                    c = cos(Pi * Grid[i]);
                    D[i] /= c;
                    W[i] *= c;
                }
            }
        } else {
                 if (numtaps % 2 == 1) {
                for (i = 0; i < gridsize; i++) {
                    c = sin(Pi2 * Grid[i]);
                    D[i] /= c;
                    W[i] *= c;
                }
            } else {
                for (i = 0; i < gridsize; i++) {
                    c = sin(Pi * Grid[i]);
                    D[i] /= c;
                    W[i] *= c;
                }
            }
        }

        /*
         * Perform the Remez Exchange algorithm
         */
        for (iter = 0; iter < MAXITERATIONS; iter++) {
            CalcParms(r, Ext, Grid, D, W, ad, x, y);
            CalcError(r, ad, x, y, gridsize, Grid, D, W, E);
            Search(r, Ext, gridsize, E);
            if (isDone(r, Ext, E) != 0) {
                break;
            }
        }
        if (iter == MAXITERATIONS) {
            System.out.println("Reached maximum iteration count.\nResults may be bad.\n");
        }

        CalcParms(r, Ext, Grid, D, W, ad, x, y);

        /*
         * Find the 'taps' of the filter for use with Frequency Sampling. If odd
         * or Negative symmetry, fix the taps according to Parks McClellan
         */
        for (i = 0; i <= numtaps / 2; i++) {
            if (symmetry == POSITIVE) {
                if (numtaps % 2 == 1) {
                    c = 1;
                } else {
                    c = cos(Pi * (double) i / numtaps);
                }
            } else {
                if (numtaps % 2 == 1) {
                    c = sin(Pi2 * (double) i / numtaps);
                } else {
                    c = sin(Pi * (double) i / numtaps);
                }
            }
            taps[i] = ComputeA((double) i / numtaps, r, ad, x, y) * c;
        }

        /*
         * Frequency sampling design with calculated taps
         */
        FreqSample(numtaps, taps, symmetry);


    }

    /**
     * *****************
     * CreateDenseGrid ================= Creates the dense grid of frequencies
     * from the specified bands. Also creates the Desired Frequency Response
     * function (D[]) and the Weight function (W[]) on that dense grid
     *
     *
     * INPUT: ------ int r - 1/2 the number of filter coefficients int numtaps -
     * Number of taps in the resulting filter int numband - Number of bands in
     * user specification double bands[] - User-specified band edges [2*numband]
     * double des[] - Desired response per band [numband] double weight[] -
     * Weight per band [numband] int symmetry - Symmetry of filter - used for
     * grid check
     *
     * OUTPUT: ------- int gridsize - Number of elements in the dense frequency
     * grid double Grid[] - Frequencies (0 to 0.5) on the dense grid [gridsize]
     * double D[] - Desired response on the dense grid [gridsize] double W[] -
     * Weight function on the dense grid [gridsize] *****************
     */
    void CreateDenseGrid(int r, int numtaps, int numband, double bands[], double des[], double weight[], int gridsize, double Grid[], double D[], double W[], int symmetry) {
        int i, j, k, band;
        double delf, lowf, highf;

        delf = 0.5 / (GRIDDENSITY * r);

        /*
         * For differentiator, hilbert, symmetry is odd and Grid[0] = max(delf,
         * band[0])
         */

        if ((symmetry == NEGATIVE) && (delf > bands[0])) {
            bands[0] = delf;
        }

        j = 0;
        for (band = 0; band < numband; band++) {
            Grid[j] = bands[2 * band];
            lowf = bands[2 * band];
            highf = bands[2 * band + 1];
            k = (int) ((highf - lowf) / delf + 0.5);/*
             * .5 for rounding
             */
            for (i = 0; i < k; i++) {
                D[j] = des[band];
                W[j] = weight[band];
                Grid[j] = lowf;
                lowf += delf;
                j++;
            }
            Grid[j - 1] = highf;
        }

        /*
         * Similar to above, if odd symmetry, last grid point can't be .5 - but,
         * if there are even taps, leave the last grid point at .5
         */

        if ((symmetry == NEGATIVE) && (Grid[gridsize - 1] > (0.5 - delf)) && (numtaps % 2 == 1)) {
            Grid[gridsize - 1] = 0.5 - delf;
        }
    }

    /**
     * ******************
     * InitialGuess ============== Places Extremal Frequencies evenly throughout
     * the dense grid.
     *
     *
     * INPUT: ------ int r - 1/2 the number of filter coefficients int gridsize
     * - Number of elements in the dense frequency grid
     *
     * OUTPUT: ------- int Ext[] - Extremal indexes to dense frequency grid
     * [r+1] ******************
     */
    void InitialGuess(int r, int Ext[], int gridsize) {
        int i;

        for (i = 0; i <= r; i++) {
            Ext[i] = i * (gridsize - 1) / r;
        }
    }

    /**
     * *********************
     * CalcParms ===========
     *
     *
     * INPUT: ------ int r - 1/2 the number of filter coefficients int Ext[] -
     * Extremal indexes to dense frequency grid [r+1] double Grid[] -
     * Frequencies (0 to 0.5) on the dense grid [gridsize] double D[] - Desired
     * response on the dense grid [gridsize] double W[] - Weight function on the
     * dense grid [gridsize]
     *
     * OUTPUT: ------- double ad[] - 'b' in Oppenheim & Schafer [r+1] double x[]
     * - [r+1] double y[] - 'C' in Oppenheim & Schafer [r+1]
     * *********************
     */
    void CalcParms(int r, int Ext[], double Grid[], double D[], double W[], double ad[], double x[], double y[]) {
        int i, j, k, ld;
        double sign, xi, delta, denom, numer;

        /*
         * * Find x[]
         */

        for (i = 0; i <= r; i++) {
            x[i] = cos(Pi2 * Grid[Ext[i]]);
        }

        /*
         * * Calculate ad[] - Oppenheim & Schafer eq 7.132
         */

        ld = (r - 1) / 15 + 1;   /*
         * Skips around to avoid round errors
         */
        for (i = 0; i <= r; i++) {
            denom = 1.0;
            xi = x[i];
            for (j = 0; j < ld; j++) {
                for (k = j; k <= r; k += ld) {
                    if (k != i) {
                        denom *= 2.0 * (xi - x[k]);
                    }
                }
            }
            if (abs(denom) < 0.00001) {
                denom = 0.00001;
            }
            ad[i] = 1.0 / denom;
        }

        /*
         * * Calculate delta - Oppenheim & Schafer eq 7.131
         */

        numer = denom = 0;
        sign = 1;
        for (i = 0; i <= r; i++) {
            numer += ad[i] * D[Ext[i]];
            denom += sign * ad[i] / W[Ext[i]];
            sign = -sign;
        }
        delta = numer / denom;
        sign = 1;

        /*
         * * Calculate y[] - Oppenheim & Schafer eq 7.133b
         */

        for (i = 0; i <= r; i++) {
            y[i] = D[Ext[i]] - sign * delta / W[Ext[i]];
            sign = -sign;
        }
    }

    /**
     * *******************
     * ComputeA ========== Using values calculated in CalcParms, ComputeA
     * calculates the actual filter response at a given frequency (freq). Uses
     * eq 7.133a from Oppenheim & Schafer.
     *
     *
     * INPUT: ------ double freq - Frequency (0 to 0.5) at which to calculate A
     * int r - 1/2 the number of filter coefficients double ad[] - 'b' in
     * Oppenheim & Schafer [r+1] double x[] - [r+1] double y[] - 'C' in
     * Oppenheim & Schafer [r+1]
     *
     * OUTPUT: ------- Returns double value of A[freq] *******************
     */
    double ComputeA(double freq, int r, double ad[], double x[], double y[]) {
        int i;
        double xc, c, denom, numer;

        denom = numer = 0;
        xc = cos(Pi2 * freq);
        for (i = 0; i <= r; i++) {
            c = xc - x[i];
            if (abs(c) < 1.0e-7) {
                numer = y[i];
                denom = 1;
                break;
            }
            c = ad[i] / c;
            denom += c;
            numer += c * y[i];
        }
        return numer / denom;
    }

    /**
     * **********************
     * CalcError =========== Calculates the Error function from the desired
     * frequency response on the dense grid (D[]), the weight function on the
     * dense grid (W[]), and the present response calculation (A[])
     *
     *
     * INPUT: ------ int r - 1/2 the number of filter coefficients double ad[] -
     * [r+1] double x[] - [r+1] double y[] - [r+1] int gridsize - Number of
     * elements in the dense frequency grid double Grid[] - Frequencies on the
     * dense grid [gridsize] double D[] - Desired response on the dense grid
     * [gridsize] double W[] - Weight function on the desnse grid [gridsize]
     *
     * OUTPUT: ------- double E[] - Error function on dense grid [gridsize]
     * **********************
     */
    void CalcError(int r, double ad[], double x[], double y[], int gridsize, double Grid[], double D[], double W[], double E[]) {
        int i;
        double A;
        double max = 0, min = 0;
        this.E = new double[E.length];
        for (i = 0; i < gridsize; i++) {
            A = ComputeA(Grid[i], r, ad, x, y);
            E[i] = W[i] * (D[i] - A);
            this.E[i] = E[i];
//            max = Math.max(max, E[i]);
//            min = Math.min(min, E[i]);
//            System.out.println("E["+i+"]= "+E[i]);
        }
//        maxE = max;
//        System.out.println("max: " + max);
//        System.out.println("min: " + min);
    }

    /**
     * **********************
     * Search ======== Searches for the maxima/minima of the error curve. If
     * more than r+1 extrema are found, it uses the following heuristic 1)
     * Adjacent non-alternating extrema deleted first. 2) If there are more than
     * one excess extrema, delete the one with the smallest error. This will
     * create a non-alternation condition that is fixed by 1). 3) If there is
     * exactly one excess extremum, delete the smaller of the first/last
     * extremum
     *
     *
     * INPUT: ------ int r - 1/2 the number of filter coefficients int Ext[] -
     * Indexes to Grid[] of extremal frequencies [r+1] int gridsize - Number of
     * elements in the dense frequency grid double E[] - Array of error values.
     * [gridsize] OUTPUT: ------- int Ext[] - New indexes to extremal
     * frequencies [r+1] **********************
     */
    void Search(int r, int Ext[], int gridsize, double E[]) {
        int i, j, k, l, extra;     /*
         * Counters
         */
        int up, alt;
        int[] foundExt;             /*
         * Array of found extremals
         */

        /*
         * * Allocate enough space for found extremals.
         */

        foundExt = new int[2 * r];
        k = 0;

        /*
         * * Check for extremum at 0.
         */

        if (((E[0] > 0.0) && (E[0] > E[1]))
                || ((E[0] < 0.0) && (E[0] < E[1]))) {
            foundExt[k++] = 0;
        }

        /*
         * * Check for extrema inside dense grid
         */
        for (i = 1; i < gridsize - 1; i++) {
            if (((E[i] >= E[i - 1]) && (E[i] > E[i + 1]) && (E[i] > 0.0))
                    || ((E[i] <= E[i - 1]) && (E[i] < E[i + 1]) && (E[i] < 0.0))) {
                foundExt[k++] = i;
            }
        }

        /*
         * * Check for extremum at 0.5
         */

        j = gridsize - 1;
        if (((E[j] > 0.0) && (E[j] > E[j - 1]))
                || ((E[j] < 0.0) && (E[j] < E[j - 1]))) {
            foundExt[k++] = j;
        }


        /*
         * * Remove extra extremals
         */

        extra = k - (r + 1);

        while (extra > 0) {
            if (E[foundExt[0]] > 0.0) {
                up = 1;                /*
                 * first one is a maxima
                 */
            } else {
                up = 0;                /*
                 * first one is a minima
                 */
            }

            l = 0;
            alt = 1;
            for (j = 1; j < k; j++) {
                if (abs(E[foundExt[j]]) < abs(E[foundExt[l]])) {
                    l = j;               /*
                     * new smallest error.
                     */
                }
                if ((up != 0) && (E[foundExt[j]] < 0.0)) {
                    up = 0;             /*
                     * switch to a minima
                     */
                } else if ((up == 0) && (E[foundExt[j]] > 0.0)) {
                    up = 1;             /*
                     * switch to a maxima
                     */
                } else {
                    alt = 0;
                    break;        /*
                     * Ooops, found two non-alternating
                     */
                }               /*
                 * extrema. Delete smallest of them
                 */
            }  /*
             * if the loop finishes, all extrema are alternating
             */

            /*
             * If there's only one extremal and all are alternating, delete the
             * smallest of the first/last extremals.
             */
            if ((alt != 0) && (extra == 1)) {
                if (abs(E[foundExt[k - 1]]) < abs(E[foundExt[0]])) {
                    l = foundExt[k - 1];   /*
                     * Delete last extremal
                     */
                } else {
                    l = foundExt[0];     /*
                     * Delete first extremal
                     */
                }
            }

            for (j = l; j < k; j++) /*
             * Loop that does the deletion
             */ {
                foundExt[j] = foundExt[j + 1];
            }
            k--;
            extra--;
        }

        for (i = 0; i <= r; i++) {
            Ext[i] = foundExt[i];
            System.out.println("E["+i+"]= "+E[Ext[i]]);
            /*
             * Copy found extremals to Ext[]
             */
        }
    }
    

    /**
     * *******************
     * FreqSample ============ Simple frequency sampling algorithm to determine
     * the impulse response h[] from A's found in ComputeA
     *
     *
     * INPUT: ------ int N - Number of filter coefficients double A[] - Sample
     * points of desired response [N/2] int symmetry - Symmetry of desired
     * filter
     *
     * OUTPUT: ------- double h[] - Impulse Response of final filter [N]
     * *******************
     */
    void FreqSample(int N, double A[], int symm) {
        int n, k;
        double x, val, M;

        M = (N - 1.0) / 2.0;
        if (symm == POSITIVE) {
            if (N % 2 != 0) {
                for (n = 0; n < N; n++) {
                    val = A[0];
                    x = Pi2 * (n - M) / N;
                    for (k = 1; k <= M; k++) {
                        val += 2.0 * A[k] * cos(x * k);
                    }
                    h[n] = val / N;
                }
            } else {
                for (n = 0; n < N; n++) {
                    val = A[0];
                    x = Pi2 * (n - M) / N;
                    for (k = 1; k <= (N / 2 - 1); k++) {
                        val += 2.0 * A[k] * cos(x * k);
                    }
                    h[n] = val / N;
                }
            }
        } else {
            if (N % 2 != 0) {
                for (n = 0; n < N; n++) {
                    val = 0;
                    x = Pi2 * (n - M) / N;
                    for (k = 1; k <= M; k++) {
                        val += 2.0 * A[k] * sin(x * k);
                    }
                    h[n] = val / N;
                }
            } else {
                for (n = 0; n < N; n++) {
                    val = A[N / 2] * sin(Pi * (n - M));
                    x = Pi2 * (n - M) / N;
                    for (k = 1; k <= (N / 2 - 1); k++) {
                        val += 2.0 * A[k] * sin(x * k);
                    }
                    h[n] = val / N;
                }
            }
        }
    }

    /**
     * *****************
     * isDone ======== Checks to see if the error function is small enough to
     * consider the result to have converged.
     *
     * INPUT: ------ int r - 1/2 the number of filter coeffiecients int Ext[] -
     * Indexes to extremal frequencies [r+1] double E[] - Error function on the
     * dense grid [gridsize]
     *
     * OUTPUT: ------- Returns 1 if the result converged Returns 0 if the result
     * has not converged ******************
     */
    short isDone(int r, int Ext[], double E[]) {
        int i;
        double min, max, current;

        min = max = abs(E[Ext[0]]);
        for (i = 1; i <= r; i++) {
            current = abs(E[Ext[i]]);
            if (current < min) {
                min = current;
            }
            if (current > max) {
                max = current;
            }
        }
        if (((max - min) / max) < 0.0001) {
            return 1;
        }
        return 0;
    }

    double sin(double a) {
        return Math.sin(a);
    }

    double cos(double a){
        return Math.cos(a);
    }

    double abs(double a) {
        return Math.abs(a);
    }    
    
}
